//
// File: uav_track_ugv.cpp
//
// Code generated for Simulink model 'uav_track_ugv'.
//
// Model version                  : 1.6
// Simulink Coder version         : 9.7 (R2022a) 13-Nov-2021
// C/C++ source code generated on : Mon May  6 21:07:03 2024
//
// Target selection: ert.tlc
// Embedded hardware selection: Generic->Unspecified (assume 32-bit Generic)
// Code generation objectives: Unspecified
// Validation result: Not run
//
#include "uav_track_ugv.h"
#include "rtwtypes.h"
#include <math.h>
#include "uav_track_ugv_private.h"

extern "C" {

#include "rt_nonfinite.h"

}
// Block signals (default storage)
  B_uav_track_ugv_T uav_track_ugv_B;

// Block states (default storage)
DW_uav_track_ugv_T uav_track_ugv_DW;

// Real-time model
RT_MODEL_uav_track_ugv_T uav_track_ugv_M_ = RT_MODEL_uav_track_ugv_T();
RT_MODEL_uav_track_ugv_T *const uav_track_ugv_M = &uav_track_ugv_M_;
static void rate_scheduler(void);

//
//         This function updates active task flag for each subrate.
//         The function is called at model base rate, hence the
//         generated code self-manages all its subrates.
//
static void rate_scheduler(void)
{
  // Compute which subrates run during the next base time step.  Subrates
  //  are an integer multiple of the base rate counter.  Therefore, the subtask
  //  counter is reset when it reaches its limit (zero means run).

  (uav_track_ugv_M->Timing.TaskCounters.TID[1])++;
  if ((uav_track_ugv_M->Timing.TaskCounters.TID[1]) > 1) {// Sample time: [0.02s, 0.0s] 
    uav_track_ugv_M->Timing.TaskCounters.TID[1] = 0;
  }
}

real_T rt_urand_Upu32_Yd_f_pw_snf(uint32_T *u)
{
  uint32_T hi;
  uint32_T lo;

  // Uniform random number generator (random number between 0 and 1)

  // #define IA      16807                      magic multiplier = 7^5
  // #define IM      2147483647                 modulus = 2^31-1
  // #define IQ      127773                     IM div IA
  // #define IR      2836                       IM modulo IA
  // #define S       4.656612875245797e-10      reciprocal of 2^31-1
  // test = IA * (seed % IQ) - IR * (seed/IQ)
  // seed = test < 0 ? (test + IM) : test
  // return (seed*S)

  lo = *u % 127773U * 16807U;
  hi = *u / 127773U * 2836U;
  if (lo < hi) {
    *u = 2147483647U - (hi - lo);
  } else {
    *u = lo - hi;
  }

  return static_cast<real_T>(*u) * 4.6566128752457969E-10;
}

real_T rt_nrand_Upu32_Yd_f_pw_snf(uint32_T *u)
{
  real_T si;
  real_T sr;
  real_T y;

  // Normal (Gaussian) random number generator
  do {
    sr = 2.0 * rt_urand_Upu32_Yd_f_pw_snf(u) - 1.0;
    si = 2.0 * rt_urand_Upu32_Yd_f_pw_snf(u) - 1.0;
    si = sr * sr + si * si;
  } while (si > 1.0);

  y = sqrt(-2.0 * log(si) / si) * sr;
  return y;
}

// Model step function
void uav_track_ugv_step(void)
{
  real_T rtb_IntegralGain_idx_0;
  real_T rtb_Saturation;
  real_T rtb_Sum1_idx_0;
  real_T rtb_Sum_idx_0;
  int8_T rtb_IntegralGain_idx_0_0;
  int8_T rtb_Switch1_idx_0;
  int8_T rtb_Switch1_idx_1;
  boolean_T rtb_RelationalOperator_idx_0;
  boolean_T rtb_RelationalOperator_idx_1;

  // Outputs for Atomic SubSystem: '<Root>/Subscribe1'
  // MATLABSystem: '<S9>/SourceBlock'
  rtb_RelationalOperator_idx_0 = Sub_uav_track_ugv_20.getLatestMessage
    (&uav_track_ugv_B.b_varargout_2);

  // Outputs for Enabled SubSystem: '<S9>/Enabled Subsystem' incorporates:
  //   EnablePort: '<S61>/Enable'

  if (rtb_RelationalOperator_idx_0) {
    // SignalConversion generated from: '<S61>/In1'
    uav_track_ugv_B.In1 = uav_track_ugv_B.b_varargout_2;
  }

  // End of MATLABSystem: '<S9>/SourceBlock'
  // End of Outputs for SubSystem: '<S9>/Enabled Subsystem'
  // End of Outputs for SubSystem: '<Root>/Subscribe1'
  if (uav_track_ugv_M->Timing.TaskCounters.TID[1] == 0) {
    // Gain: '<S1>/Output' incorporates:
    //   RandomNumber: '<S1>/White Noise'

    uav_track_ugv_B.Output = sqrt(uav_track_ugv_P.BandLimitedWhiteNoise_Cov) /
      0.1414213562373095 * uav_track_ugv_DW.NextOutput;
  }

  // Outputs for Atomic SubSystem: '<Root>/Subscribe'
  // MATLABSystem: '<S8>/SourceBlock'
  rtb_RelationalOperator_idx_0 = Sub_uav_track_ugv_16.getLatestMessage
    (&uav_track_ugv_B.b_varargout_2);

  // Outputs for Enabled SubSystem: '<S8>/Enabled Subsystem' incorporates:
  //   EnablePort: '<S60>/Enable'

  if (rtb_RelationalOperator_idx_0) {
    // SignalConversion generated from: '<S60>/In1'
    uav_track_ugv_B.In1_i = uav_track_ugv_B.b_varargout_2;
  }

  // End of MATLABSystem: '<S8>/SourceBlock'
  // End of Outputs for SubSystem: '<S8>/Enabled Subsystem'
  // End of Outputs for SubSystem: '<Root>/Subscribe'
  if (uav_track_ugv_M->Timing.TaskCounters.TID[1] == 0) {
    // Gain: '<S2>/Output' incorporates:
    //   RandomNumber: '<S2>/White Noise'

    uav_track_ugv_B.Output_k = sqrt(uav_track_ugv_P.BandLimitedWhiteNoise1_Cov) /
      0.1414213562373095 * uav_track_ugv_DW.NextOutput_j;
  }

  // Sum: '<Root>/Sum' incorporates:
  //   SignalConversion generated from: '<Root>/Vector Concatenate1'
  //   SignalConversion generated from: '<Root>/Vector Concatenate'
  //   Sum: '<Root>/Sum1'
  //   Sum: '<Root>/Sum2'

  uav_track_ugv_B.rtb_Sum_c = (uav_track_ugv_B.In1.Pose.Position.X +
    uav_track_ugv_B.Output) - (uav_track_ugv_B.In1_i.Pose.Position.X +
    uav_track_ugv_B.Output_k);

  // Gain: '<S46>/Filter Coefficient' incorporates:
  //   DiscreteIntegrator: '<S38>/Filter'
  //   Gain: '<S37>/Derivative Gain'
  //   Sum: '<S38>/SumD'

  uav_track_ugv_B.rtb_Sum1_m = (uav_track_ugv_P.DiscretePIDController_D *
    uav_track_ugv_B.rtb_Sum_c - uav_track_ugv_DW.Filter_DSTATE[0]) *
    uav_track_ugv_P.DiscretePIDController_N;

  // Sum: '<S52>/Sum' incorporates:
  //   DiscreteIntegrator: '<S43>/Integrator'
  //   Gain: '<S48>/Proportional Gain'

  uav_track_ugv_B.rtb_Saturation_k = (uav_track_ugv_P.DiscretePIDController_P *
    uav_track_ugv_B.rtb_Sum_c + uav_track_ugv_DW.Integrator_DSTATE[0]) +
    uav_track_ugv_B.rtb_Sum1_m;

  // DeadZone: '<S36>/DeadZone'
  if (uav_track_ugv_B.rtb_Saturation_k >
      uav_track_ugv_P.DiscretePIDController_UpperSatu) {
    uav_track_ugv_B.rtb_IntegralGain_c = uav_track_ugv_B.rtb_Saturation_k -
      uav_track_ugv_P.DiscretePIDController_UpperSatu;
  } else if (uav_track_ugv_B.rtb_Saturation_k >=
             uav_track_ugv_P.DiscretePIDController_LowerSatu) {
    uav_track_ugv_B.rtb_IntegralGain_c = 0.0;
  } else {
    uav_track_ugv_B.rtb_IntegralGain_c = uav_track_ugv_B.rtb_Saturation_k -
      uav_track_ugv_P.DiscretePIDController_LowerSatu;
  }

  // RelationalOperator: '<S34>/Relational Operator' incorporates:
  //   Constant: '<S34>/Constant5'

  rtb_RelationalOperator_idx_0 = (uav_track_ugv_P.Constant5_Value !=
    uav_track_ugv_B.rtb_IntegralGain_c);

  // Switch: '<S34>/Switch1' incorporates:
  //   Constant: '<S34>/Constant'
  //   Constant: '<S34>/Constant2'
  //   Constant: '<S34>/Constant5'
  //   RelationalOperator: '<S34>/fix for DT propagation issue'

  if (uav_track_ugv_B.rtb_IntegralGain_c > uav_track_ugv_P.Constant5_Value) {
    rtb_Switch1_idx_0 = uav_track_ugv_P.Constant_Value_jg;
  } else {
    rtb_Switch1_idx_0 = uav_track_ugv_P.Constant2_Value;
  }

  // Saturate: '<S50>/Saturation'
  if (uav_track_ugv_B.rtb_Saturation_k >
      uav_track_ugv_P.DiscretePIDController_UpperSatu) {
    rtb_Saturation = uav_track_ugv_P.DiscretePIDController_UpperSatu;
  } else if (uav_track_ugv_B.rtb_Saturation_k <
             uav_track_ugv_P.DiscretePIDController_LowerSatu) {
    rtb_Saturation = uav_track_ugv_P.DiscretePIDController_LowerSatu;
  } else {
    rtb_Saturation = uav_track_ugv_B.rtb_Saturation_k;
  }

  // Gain: '<S46>/Filter Coefficient'
  rtb_Sum1_idx_0 = uav_track_ugv_B.rtb_Sum1_m;

  // Sum: '<Root>/Sum'
  rtb_Sum_idx_0 = uav_track_ugv_B.rtb_Sum_c;

  // DeadZone: '<S36>/DeadZone' incorporates:
  //   Gain: '<S40>/Integral Gain'

  rtb_IntegralGain_idx_0 = uav_track_ugv_P.DiscretePIDController_I *
    uav_track_ugv_B.rtb_Sum_c;

  // Sum: '<Root>/Sum' incorporates:
  //   SignalConversion generated from: '<Root>/Vector Concatenate1'
  //   SignalConversion generated from: '<Root>/Vector Concatenate'
  //   Sum: '<Root>/Sum1'
  //   Sum: '<Root>/Sum2'

  uav_track_ugv_B.rtb_Sum_c = (uav_track_ugv_B.In1.Pose.Position.Y +
    uav_track_ugv_B.Output) - (uav_track_ugv_B.In1_i.Pose.Position.Y +
    uav_track_ugv_B.Output_k);

  // Gain: '<S46>/Filter Coefficient' incorporates:
  //   DiscreteIntegrator: '<S38>/Filter'
  //   Gain: '<S37>/Derivative Gain'
  //   Sum: '<S38>/SumD'

  uav_track_ugv_B.rtb_Sum1_m = (uav_track_ugv_P.DiscretePIDController_D *
    uav_track_ugv_B.rtb_Sum_c - uav_track_ugv_DW.Filter_DSTATE[1]) *
    uav_track_ugv_P.DiscretePIDController_N;

  // Sum: '<S52>/Sum' incorporates:
  //   DiscreteIntegrator: '<S43>/Integrator'
  //   Gain: '<S48>/Proportional Gain'

  uav_track_ugv_B.rtb_Saturation_k = (uav_track_ugv_P.DiscretePIDController_P *
    uav_track_ugv_B.rtb_Sum_c + uav_track_ugv_DW.Integrator_DSTATE[1]) +
    uav_track_ugv_B.rtb_Sum1_m;

  // DeadZone: '<S36>/DeadZone'
  if (uav_track_ugv_B.rtb_Saturation_k >
      uav_track_ugv_P.DiscretePIDController_UpperSatu) {
    uav_track_ugv_B.rtb_IntegralGain_c = uav_track_ugv_B.rtb_Saturation_k -
      uav_track_ugv_P.DiscretePIDController_UpperSatu;
  } else if (uav_track_ugv_B.rtb_Saturation_k >=
             uav_track_ugv_P.DiscretePIDController_LowerSatu) {
    uav_track_ugv_B.rtb_IntegralGain_c = 0.0;
  } else {
    uav_track_ugv_B.rtb_IntegralGain_c = uav_track_ugv_B.rtb_Saturation_k -
      uav_track_ugv_P.DiscretePIDController_LowerSatu;
  }

  // RelationalOperator: '<S34>/Relational Operator' incorporates:
  //   Constant: '<S34>/Constant5'

  rtb_RelationalOperator_idx_1 = (uav_track_ugv_P.Constant5_Value !=
    uav_track_ugv_B.rtb_IntegralGain_c);

  // Switch: '<S34>/Switch1' incorporates:
  //   Constant: '<S34>/Constant'
  //   Constant: '<S34>/Constant2'
  //   Constant: '<S34>/Constant5'
  //   RelationalOperator: '<S34>/fix for DT propagation issue'

  if (uav_track_ugv_B.rtb_IntegralGain_c > uav_track_ugv_P.Constant5_Value) {
    rtb_Switch1_idx_1 = uav_track_ugv_P.Constant_Value_jg;
  } else {
    rtb_Switch1_idx_1 = uav_track_ugv_P.Constant2_Value;
  }

  // Gain: '<S40>/Integral Gain'
  uav_track_ugv_B.rtb_IntegralGain_c = uav_track_ugv_P.DiscretePIDController_I *
    uav_track_ugv_B.rtb_Sum_c;

  // BusAssignment: '<Root>/Bus Assignment1' incorporates:
  //   Constant: '<Root>/Constant3'
  //   Constant: '<Root>/Constant4'
  //   Constant: '<S4>/Constant'

  uav_track_ugv_B.BusAssignment1 = uav_track_ugv_P.Constant_Value;
  uav_track_ugv_B.BusAssignment1.CoordinateFrame =
    uav_track_ugv_P.Constant3_Value_a;
  uav_track_ugv_B.BusAssignment1.TypeMask = uav_track_ugv_P.Constant4_Value;
  uav_track_ugv_B.BusAssignment1.Velocity.X = rtb_Saturation;

  // Saturate: '<S50>/Saturation'
  if (uav_track_ugv_B.rtb_Saturation_k >
      uav_track_ugv_P.DiscretePIDController_UpperSatu) {
    // BusAssignment: '<Root>/Bus Assignment1'
    uav_track_ugv_B.BusAssignment1.Velocity.Y =
      uav_track_ugv_P.DiscretePIDController_UpperSatu;
  } else if (uav_track_ugv_B.rtb_Saturation_k <
             uav_track_ugv_P.DiscretePIDController_LowerSatu) {
    // BusAssignment: '<Root>/Bus Assignment1'
    uav_track_ugv_B.BusAssignment1.Velocity.Y =
      uav_track_ugv_P.DiscretePIDController_LowerSatu;
  } else {
    // BusAssignment: '<Root>/Bus Assignment1'
    uav_track_ugv_B.BusAssignment1.Velocity.Y = uav_track_ugv_B.rtb_Saturation_k;
  }

  // BusAssignment: '<Root>/Bus Assignment1' incorporates:
  //   Constant: '<Root>/Constant'

  uav_track_ugv_B.BusAssignment1.Position.Z = uav_track_ugv_P.Constant_Value_a;

  // Outputs for Atomic SubSystem: '<Root>/Publish2'
  // MATLABSystem: '<S7>/SinkBlock'
  Pub_uav_track_ugv_9.publish(&uav_track_ugv_B.BusAssignment1);

  // End of Outputs for SubSystem: '<Root>/Publish2'

  // BusAssignment: '<Root>/Bus Assignment' incorporates:
  //   Constant: '<S3>/Constant'

  uav_track_ugv_B.BusAssignment = uav_track_ugv_P.Constant_Value_f;
  uav_track_ugv_B.BusAssignment.Position.X = rtb_Sum_idx_0;
  uav_track_ugv_B.BusAssignment.Position.Y = uav_track_ugv_B.rtb_Sum_c;

  // Outputs for Atomic SubSystem: '<Root>/Publish1'
  // MATLABSystem: '<S6>/SinkBlock'
  Pub_uav_track_ugv_46.publish(&uav_track_ugv_B.BusAssignment);

  // End of Outputs for SubSystem: '<Root>/Publish1'

  // Switch: '<S34>/Switch2' incorporates:
  //   Constant: '<S34>/Constant3'
  //   Constant: '<S34>/Constant4'
  //   Constant: '<S34>/Constant5'
  //   RelationalOperator: '<S34>/fix for DT propagation issue1'

  if (rtb_IntegralGain_idx_0 > uav_track_ugv_P.Constant5_Value) {
    rtb_IntegralGain_idx_0_0 = uav_track_ugv_P.Constant3_Value;
  } else {
    rtb_IntegralGain_idx_0_0 = uav_track_ugv_P.Constant4_Value_c;
  }

  // Switch: '<S34>/Switch' incorporates:
  //   Constant: '<S34>/Constant1'
  //   Logic: '<S34>/AND3'
  //   RelationalOperator: '<S34>/Equal1'

  if (rtb_RelationalOperator_idx_0 && (rtb_Switch1_idx_0 ==
       rtb_IntegralGain_idx_0_0)) {
    rtb_IntegralGain_idx_0 = uav_track_ugv_P.Constant1_Value;
  }

  // Update for DiscreteIntegrator: '<S43>/Integrator'
  uav_track_ugv_DW.Integrator_DSTATE[0] += uav_track_ugv_P.Integrator_gainval *
    rtb_IntegralGain_idx_0;

  // Switch: '<S34>/Switch2' incorporates:
  //   Constant: '<S34>/Constant3'
  //   Constant: '<S34>/Constant4'
  //   Constant: '<S34>/Constant5'
  //   RelationalOperator: '<S34>/fix for DT propagation issue1'

  if (uav_track_ugv_B.rtb_IntegralGain_c > uav_track_ugv_P.Constant5_Value) {
    rtb_Switch1_idx_0 = uav_track_ugv_P.Constant3_Value;
  } else {
    rtb_Switch1_idx_0 = uav_track_ugv_P.Constant4_Value_c;
  }

  // Switch: '<S34>/Switch' incorporates:
  //   Constant: '<S34>/Constant1'
  //   Logic: '<S34>/AND3'
  //   RelationalOperator: '<S34>/Equal1'

  if (rtb_RelationalOperator_idx_1 && (rtb_Switch1_idx_1 == rtb_Switch1_idx_0))
  {
    uav_track_ugv_B.rtb_IntegralGain_c = uav_track_ugv_P.Constant1_Value;
  }

  // Update for DiscreteIntegrator: '<S43>/Integrator'
  uav_track_ugv_DW.Integrator_DSTATE[1] += uav_track_ugv_P.Integrator_gainval *
    uav_track_ugv_B.rtb_IntegralGain_c;
  if (uav_track_ugv_M->Timing.TaskCounters.TID[1] == 0) {
    // Update for RandomNumber: '<S1>/White Noise'
    uav_track_ugv_DW.NextOutput = rt_nrand_Upu32_Yd_f_pw_snf
      (&uav_track_ugv_DW.RandSeed) * uav_track_ugv_P.WhiteNoise_StdDev +
      uav_track_ugv_P.WhiteNoise_Mean;

    // Update for RandomNumber: '<S2>/White Noise'
    uav_track_ugv_DW.NextOutput_j = rt_nrand_Upu32_Yd_f_pw_snf
      (&uav_track_ugv_DW.RandSeed_m) * uav_track_ugv_P.WhiteNoise_StdDev_m +
      uav_track_ugv_P.WhiteNoise_Mean_p;
  }

  // Update for DiscreteIntegrator: '<S38>/Filter'
  uav_track_ugv_DW.Filter_DSTATE[0] += uav_track_ugv_P.Filter_gainval *
    rtb_Sum1_idx_0;
  uav_track_ugv_DW.Filter_DSTATE[1] += uav_track_ugv_P.Filter_gainval *
    uav_track_ugv_B.rtb_Sum1_m;
  rate_scheduler();
}

// Model initialize function
void uav_track_ugv_initialize(void)
{
  // Registration code

  // initialize non-finites
  rt_InitInfAndNaN(sizeof(real_T));

  {
    real_T tmp;
    int32_T r;
    int32_T t;
    uint32_T tseed;
    char_T b_zeroDelimTopic[11];
    char_T b_zeroDelimTopic_0[4];
    static const char_T tmp_0[10] = { '/', 'u', 'g', 'v', '1', '/', 'p', 'o',
      's', 'e' };

    static const char_T tmp_1[10] = { '/', 'u', 'a', 'v', '1', '/', 'p', 'o',
      's', 'e' };

    static const char_T tmp_2[31] = { '/', 'u', 'a', 'v', '1', '/', 'm', 'a',
      'v', 'r', 'o', 's', '/', 's', 'e', 't', 'p', 'o', 'i', 'n', 't', '_', 'r',
      'a', 'w', '/', 'l', 'o', 'c', 'a', 'l' };

    // InitializeConditions for DiscreteIntegrator: '<S43>/Integrator'
    uav_track_ugv_DW.Integrator_DSTATE[0] =
      uav_track_ugv_P.DiscretePIDController_Initial_e;
    uav_track_ugv_DW.Integrator_DSTATE[1] =
      uav_track_ugv_P.DiscretePIDController_Initial_e;

    // InitializeConditions for RandomNumber: '<S1>/White Noise'
    tmp = floor(uav_track_ugv_P.BandLimitedWhiteNoise_seed);
    if (rtIsNaN(tmp) || rtIsInf(tmp)) {
      tmp = 0.0;
    } else {
      tmp = fmod(tmp, 4.294967296E+9);
    }

    tseed = tmp < 0.0 ? static_cast<uint32_T>(-static_cast<int32_T>
      (static_cast<uint32_T>(-tmp))) : static_cast<uint32_T>(tmp);
    r = static_cast<int32_T>(tseed >> 16U);
    t = static_cast<int32_T>(tseed & 32768U);
    uav_track_ugv_DW.RandSeed = ((((tseed - (static_cast<uint32_T>(r) << 16U)) +
      t) << 16U) + t) + r;
    if (uav_track_ugv_DW.RandSeed < 1U) {
      uav_track_ugv_DW.RandSeed = 1144108930U;
    } else if (uav_track_ugv_DW.RandSeed > 2147483646U) {
      uav_track_ugv_DW.RandSeed = 2147483646U;
    }

    uav_track_ugv_DW.NextOutput = rt_nrand_Upu32_Yd_f_pw_snf
      (&uav_track_ugv_DW.RandSeed) * uav_track_ugv_P.WhiteNoise_StdDev +
      uav_track_ugv_P.WhiteNoise_Mean;

    // End of InitializeConditions for RandomNumber: '<S1>/White Noise'

    // InitializeConditions for RandomNumber: '<S2>/White Noise'
    tmp = floor(uav_track_ugv_P.BandLimitedWhiteNoise1_seed);
    if (rtIsNaN(tmp) || rtIsInf(tmp)) {
      tmp = 0.0;
    } else {
      tmp = fmod(tmp, 4.294967296E+9);
    }

    tseed = tmp < 0.0 ? static_cast<uint32_T>(-static_cast<int32_T>
      (static_cast<uint32_T>(-tmp))) : static_cast<uint32_T>(tmp);
    r = static_cast<int32_T>(tseed >> 16U);
    t = static_cast<int32_T>(tseed & 32768U);
    uav_track_ugv_DW.RandSeed_m = ((((tseed - (static_cast<uint32_T>(r) << 16U))
      + t) << 16U) + t) + r;
    if (uav_track_ugv_DW.RandSeed_m < 1U) {
      uav_track_ugv_DW.RandSeed_m = 1144108930U;
    } else if (uav_track_ugv_DW.RandSeed_m > 2147483646U) {
      uav_track_ugv_DW.RandSeed_m = 2147483646U;
    }

    uav_track_ugv_DW.NextOutput_j = rt_nrand_Upu32_Yd_f_pw_snf
      (&uav_track_ugv_DW.RandSeed_m) * uav_track_ugv_P.WhiteNoise_StdDev_m +
      uav_track_ugv_P.WhiteNoise_Mean_p;

    // End of InitializeConditions for RandomNumber: '<S2>/White Noise'

    // InitializeConditions for DiscreteIntegrator: '<S38>/Filter'
    uav_track_ugv_DW.Filter_DSTATE[0] =
      uav_track_ugv_P.DiscretePIDController_InitialCo;
    uav_track_ugv_DW.Filter_DSTATE[1] =
      uav_track_ugv_P.DiscretePIDController_InitialCo;

    // SystemInitialize for Atomic SubSystem: '<Root>/Subscribe1'
    // SystemInitialize for Enabled SubSystem: '<S9>/Enabled Subsystem'
    // SystemInitialize for SignalConversion generated from: '<S61>/In1' incorporates:
    //   Outport: '<S61>/Out1'

    uav_track_ugv_B.In1 = uav_track_ugv_P.Out1_Y0_l;

    // End of SystemInitialize for SubSystem: '<S9>/Enabled Subsystem'

    // Start for MATLABSystem: '<S9>/SourceBlock'
    uav_track_ugv_DW.obj_e.matlabCodegenIsDeleted = false;
    uav_track_ugv_DW.obj_e.isInitialized = 1;
    for (r = 0; r < 10; r++) {
      b_zeroDelimTopic[r] = tmp_0[r];
    }

    b_zeroDelimTopic[10] = '\x00';
    Sub_uav_track_ugv_20.createSubscriber(&b_zeroDelimTopic[0], 1);
    uav_track_ugv_DW.obj_e.isSetupComplete = true;

    // End of Start for MATLABSystem: '<S9>/SourceBlock'
    // End of SystemInitialize for SubSystem: '<Root>/Subscribe1'

    // SystemInitialize for Atomic SubSystem: '<Root>/Subscribe'
    // SystemInitialize for Enabled SubSystem: '<S8>/Enabled Subsystem'
    // SystemInitialize for SignalConversion generated from: '<S60>/In1' incorporates:
    //   Outport: '<S60>/Out1'

    uav_track_ugv_B.In1_i = uav_track_ugv_P.Out1_Y0;

    // End of SystemInitialize for SubSystem: '<S8>/Enabled Subsystem'

    // Start for MATLABSystem: '<S8>/SourceBlock'
    uav_track_ugv_DW.obj_p.matlabCodegenIsDeleted = false;
    uav_track_ugv_DW.obj_p.isInitialized = 1;
    for (r = 0; r < 10; r++) {
      b_zeroDelimTopic[r] = tmp_1[r];
    }

    b_zeroDelimTopic[10] = '\x00';
    Sub_uav_track_ugv_16.createSubscriber(&b_zeroDelimTopic[0], 1);
    uav_track_ugv_DW.obj_p.isSetupComplete = true;

    // End of Start for MATLABSystem: '<S8>/SourceBlock'
    // End of SystemInitialize for SubSystem: '<Root>/Subscribe'

    // SystemInitialize for Atomic SubSystem: '<Root>/Publish2'
    // Start for MATLABSystem: '<S7>/SinkBlock'
    uav_track_ugv_DW.obj.matlabCodegenIsDeleted = false;
    uav_track_ugv_DW.obj.isInitialized = 1;
    for (r = 0; r < 31; r++) {
      uav_track_ugv_B.b_zeroDelimTopic[r] = tmp_2[r];
    }

    uav_track_ugv_B.b_zeroDelimTopic[31] = '\x00';
    Pub_uav_track_ugv_9.createPublisher(&uav_track_ugv_B.b_zeroDelimTopic[0], 1);
    uav_track_ugv_DW.obj.isSetupComplete = true;

    // End of Start for MATLABSystem: '<S7>/SinkBlock'
    // End of SystemInitialize for SubSystem: '<Root>/Publish2'

    // SystemInitialize for Atomic SubSystem: '<Root>/Publish1'
    // Start for MATLABSystem: '<S6>/SinkBlock'
    uav_track_ugv_DW.obj_h.matlabCodegenIsDeleted = false;
    uav_track_ugv_DW.obj_h.isInitialized = 1;
    b_zeroDelimTopic_0[0] = '/';
    b_zeroDelimTopic_0[1] = 'e';
    b_zeroDelimTopic_0[2] = '1';
    b_zeroDelimTopic_0[3] = '\x00';
    Pub_uav_track_ugv_46.createPublisher(&b_zeroDelimTopic_0[0], 1);
    uav_track_ugv_DW.obj_h.isSetupComplete = true;

    // End of SystemInitialize for SubSystem: '<Root>/Publish1'
  }
}

// Model terminate function
void uav_track_ugv_terminate(void)
{
  // Terminate for Atomic SubSystem: '<Root>/Subscribe1'
  // Terminate for MATLABSystem: '<S9>/SourceBlock'
  if (!uav_track_ugv_DW.obj_e.matlabCodegenIsDeleted) {
    uav_track_ugv_DW.obj_e.matlabCodegenIsDeleted = true;
  }

  // End of Terminate for MATLABSystem: '<S9>/SourceBlock'
  // End of Terminate for SubSystem: '<Root>/Subscribe1'

  // Terminate for Atomic SubSystem: '<Root>/Subscribe'
  // Terminate for MATLABSystem: '<S8>/SourceBlock'
  if (!uav_track_ugv_DW.obj_p.matlabCodegenIsDeleted) {
    uav_track_ugv_DW.obj_p.matlabCodegenIsDeleted = true;
  }

  // End of Terminate for MATLABSystem: '<S8>/SourceBlock'
  // End of Terminate for SubSystem: '<Root>/Subscribe'

  // Terminate for Atomic SubSystem: '<Root>/Publish2'
  // Terminate for MATLABSystem: '<S7>/SinkBlock'
  if (!uav_track_ugv_DW.obj.matlabCodegenIsDeleted) {
    uav_track_ugv_DW.obj.matlabCodegenIsDeleted = true;
  }

  // End of Terminate for MATLABSystem: '<S7>/SinkBlock'
  // End of Terminate for SubSystem: '<Root>/Publish2'

  // Terminate for Atomic SubSystem: '<Root>/Publish1'
  // Terminate for MATLABSystem: '<S6>/SinkBlock'
  if (!uav_track_ugv_DW.obj_h.matlabCodegenIsDeleted) {
    uav_track_ugv_DW.obj_h.matlabCodegenIsDeleted = true;
  }

  // End of Terminate for MATLABSystem: '<S6>/SinkBlock'
  // End of Terminate for SubSystem: '<Root>/Publish1'
}

//
// File trailer for generated code.
//
// [EOF]
//
